//
// Created by T on 2018/11/16.
//

#include "led.h"
#include <stdarg.h>
#include "xtime.h"

typedef struct {
    GPIO_TypeDef *GPIOx;
    uint16_t GPIO_Pin;
} led_gpio_t;

led_gpio_t led_gpio[LED_MAX_LEDS] = {
        {GPIOB, GPIO_PIN_5}, // LED_0
        {GPIOE, GPIO_PIN_5}, // LED_1
};


typedef struct {
    uint8_t mode;       /* Operation mode (TOGGLE, ON, OFF) */
    uint8_t todo;       /* Blink cycles left */
    uint16_t onTim;     /* On milliseconds in On/off cycle */
    uint16_t time;      /* On/off cycle time (msec) */
    timev_t next;     /* Time for next change */
} led_ctl_t;

led_ctl_t led_ctl_tab[LED_MAX_LEDS];

static uint8_t ledState; // LED state at last set/clr/blink update
static uint8_t preBlinkState; // Original State before going to blink mode

/**
  * @brief  set LED ON/OFF
  * @param  leds: bit mask value of LEDs (LED_0, LED_1, ...)
  * @param  mode: which LED mode (LED_MODE_OFF, LED_MODE_ON)
  * @retval None
  */
void led_raw_on_off(uint8_t leds, uint8_t mode)
{
    uint8_t led;
    led_gpio_t *gpio;

    mode &= LED_MODE_ON;

    for (led = 0, gpio = led_gpio; led < LED_MAX_LEDS; ++gpio, ++led) {
        if ((leds & 1U << led) && gpio->GPIOx) {
            HAL_GPIO_WritePin(gpio->GPIOx, gpio->GPIO_Pin, mode == LED_MODE_ON ? GPIO_PIN_RESET : GPIO_PIN_SET);
        }
    }

    /* Remember current state */
    if (mode) {
        ledState |= leds;
    } else {
        ledState &= (leds ^ 0xFF);
    }
}

static void led_blink(uint8_t leds, uint16_t period, uint16_t lightTime, uint8_t numBlinks)
{
    uint8_t led;
    led_ctl_t *ctl;

    leds &= LED_ALL;
    if (!leds)
        return;

    if (!lightTime || !period) {
        led_raw_on_off(leds, LED_MODE_OFF); /* No on time, turn off */
        return;
    }

    if (lightTime >= period) {
        led_raw_on_off(leds, LED_MODE_ON); /* >= 100%, turn on */
        return;
    }

    ctl = led_ctl_tab;
    for (led = LED_0; leds; ++ctl, led <<= 1) {
        if (!(leds & led))
            continue;
        /* Store the current state of the led before going to blinking if not already blinking */
        if(ctl->mode < LED_MODE_BLINK)
            preBlinkState |= (led & ledState);

        ctl->mode = LED_MODE_OFF; /* Stop previous blink */
        ctl->time = period; /* Time for one on/off cycle */
        ctl->onTim = lightTime; /* Time of cycle LED is on */
        ctl->todo  = numBlinks; /* Number of blink cycles */
        if (!numBlinks)
            ctl->mode |= LED_MODE_FLASH;  /* Continuous */
        gettimev(&ctl->next); /* Start now */
        ctl->mode |= LED_MODE_BLINK; /* Enable blinking */
        leds ^= led;
    }
}

static void led_flash(uint8_t leds, va_list va)
{
    uint16_t period = (uint16_t) va_arg(va, int);
    uint16_t lightTime = (uint16_t) va_arg(va, int);
    uint8_t numBlinks = (uint8_t) va_arg(va, int);
    led_blink(leds, period, lightTime, numBlinks);
}

/**
  * @brief  set LED status
  * @param  leds: bit mask value of LEDs (LED_0, LED_1, ...)
  * @param  mode: which LED mode (LED_MODE_OFF, LED_MODE_ON, LED_MODE_BLINK, LED_MODE_FLASH, LED_MODE_TOGGLE)
  * @param  ...: for LED_MODE_FLASH, here have 3 additional param: period, lightTime and numBlinks.
  *              uint16_t period: length of each cycle in milliseconds
  *              uint16_t lightTime: the milliseconds in each period where the led will be on
  *              uint8_t numBlinks: number of blinks, (0 means forever)
  * @retval None
  */
void led_set(uint8_t leds, uint8_t mode, ...)
{
    uint8_t led;
    led_ctl_t *ctl;
    va_list va;

    switch (mode) {
        case LED_MODE_BLINK:
            /* Default blink, 1 time, D% duty cycle */
            led_blink(leds, LED_DEFAULT_FLASH_TIME, LED_DEFAULT_DUTY_CYCLE, 1);
            break;

        case LED_MODE_FLASH:
            va_start(va, mode);
            led_flash(leds, va);
            va_end(va);
            break;

        case LED_MODE_ON:
        case LED_MODE_OFF:
        case LED_MODE_TOGGLE:
            ctl = led_ctl_tab;
            for (leds &= LED_ALL, led = LED_0; leds; ++ctl, led <<= 1) {
                if (!(leds & led))
                    continue;
                if (mode == LED_MODE_TOGGLE) {
                    ctl->mode ^= LED_MODE_ON; /* Toggle */
                } else {
                    ctl->mode = mode; /* ON or OFF */
                }
                led_raw_on_off(led, ctl->mode);
                leds ^= led;
            }
            break;

        default:
            break;
    }
}

uint8_t led_get_mode(uint8_t led) {
    uint8_t bitn = 0;

    led &= LED_ALL;
    if (!led)
        return 0;

    while (led >>= 1)
        ++bitn;

    if (bitn >= LED_MAX_LEDS)
        return 0;

    return led_ctl_tab[bitn].mode;
}

static void led_task_fun(void) {
    led_ctl_t *ctl;
    timev_t time;
    uint16_t wait;
    uint8_t leds;
    uint8_t led;

    gettimev(&time);
    ctl = led_ctl_tab;

    for (leds = LED_ALL, led = LED_0; leds; ++ctl, led <<= 1) {
        if (!(leds & led))
            continue;
        leds ^= led;
        if (!(ctl->mode & LED_MODE_BLINK))
            continue;
        if (timecmp(&time, &ctl->next, <))
            continue;

        if (ctl->mode & LED_MODE_ON) {
            wait = ctl->time - ctl->onTim; /* time of cycle for off */
            ctl->mode &= ~LED_MODE_ON; /* Say it's not on */
            led_raw_on_off(led, LED_MODE_OFF); /* Turn it off */
            if (!(ctl->mode & LED_MODE_FLASH)) {
                ctl->todo--; /* Not continuous, reduce count */
            }
        } else if ((!ctl->todo) && !(ctl->mode & LED_MODE_FLASH)) {
            ctl->mode ^= LED_MODE_BLINK; /* No more blinks */
        } else {
            wait = ctl->onTim; /* time of cycle for on */
            ctl->mode |= LED_MODE_ON; /* Say it's on */
            led_raw_on_off(led, LED_MODE_ON); /* Turn it on */
        }

        if (ctl->mode & LED_MODE_BLINK) {
            timevaddmsec(&time, wait, &ctl->next);
        } else {
            /* After blinking, set the LED back to the state before it blinks */
            led_set(led, (preBlinkState & led) ? LED_MODE_ON : LED_MODE_OFF);
            /* Clear the saved bit */
            preBlinkState &= (led ^ 0xFF);
        }
    }
}


task_t led_task = {
        TASK_FLAG_STAT_READY,
        led_task_fun,
};
